/*
 * main.c
 */
#include "System\F2806x_Device.h"
#include <stdio.h>
#include <string.h>
#include "Drivers\debugLED.h"
#include "Drivers\OrdOutputs.h"
#include "Drivers\PwrLEDs.h"
#include "Drivers\LEDStrips.h"
#include "Drivers\RangeAndPwrMon.h"
#include "Drivers\UARTDriverDBG.h"
#include "Drivers\PWMDriver.h"
#include "Ctrl\Ctrl.h"
#include "Timer.h"
#include "IntTimer.h"
#include "CANDriver.h"
#include "DataFile.h"
#include "Util_DLOG4CH.h"
// use floating point math!
#define MATH_TYPE 1
#include "IQmathLib.h"

extern "C" void InitSysCtrl(void);

// flash support
#define FLASH 1
extern "C" void InitFlash(void);
extern Uint16 RamfuncsLoadStart, RamfuncsLoadEnd, RamfuncsRunStart, RamfuncsLoadSize;

#define CAN_BASE 0x010000

// Drivers
DebugLED ledDebug;
OrdOutputs ordOutputs;
PwrLEDs pwrLEDs;
LEDStrips ledStrips;
RangeAndPwrMon rangPwrMon;
UARTDriverDBG uartDBG;
PWMDriver pwmDrv;

// Components
Timer timer;
IntTimer intTimer;
CANDriver canDrv;
SDataFile df;
CtrlModelClass ctrlMdl;
DLOG_4CH dlog = DLOG_4CH_DEFAULTS;
int16 dlog1, dlog2, dlog3, dlog4;


// main step time
#define STEPTIME 0.01 /* standard steptime, 100 Hz */


// RPM Data
bool RPMNextIsBlack = false;
unsigned int RPMCount = 0;
unsigned int RPMSpeedCount = 0;
#define RPMCOUNTMAX 20000
#define RPMBLACKTHREEVOLT 1.52
#define RPMWHITETHREEVOLT 1.45
// 3.3V
// BLACK
// 2.0V - RPMBLACKTHREEVOLT
////////////////////
////////////////////
////////////////////
// 1.0V - RPMWHITETHREEVOLT
// WHITE
// 0.0V
float RpmSensVolt;
float Current1FLT = 0;
extern "C" interrupt void CpuTimer1Isr(void)
{
   intTimer.m_InterruptCount++;

   float rpmSensVolt = AdcResult.ADCRESULT2*3.3f/4095.0f;
   RpmSensVolt = rpmSensVolt;
   if( RPMNextIsBlack ) // wait for black
   {
	   if( rpmSensVolt > RPMBLACKTHREEVOLT)
	   {
		   RPMNextIsBlack = false;
	   }
   }
   else // wait for white
   {
	   if( rpmSensVolt < RPMWHITETHREEVOLT)
	   {
		   RPMNextIsBlack = true;
		   RPMSpeedCount++; // increase count
	   }
   }

   RPMCount++;
   if(RPMCount > RPMCOUNTMAX)
   {
	   RPMCount = 0;
	   df.SpeedRPM1 = 60.0f * RPMSpeedCount * (20000/RPMCOUNTMAX);
	   RPMSpeedCount = 0;
   }

   // filter Current
   Current1FLT = (Current1FLT*0.99f) + (AdcResult.ADCRESULT4 * 0.01f);

   // update
   //dlog1 = RPMCount;
   //dlog2 = AdcResult.ADCRESULT4;
   //dlog.update(&dlog);

   // The CPU acknowledges the interrupt.
}

int main(void) 
{
	//=================================
	//	INITIALISATION - General
	//=================================
	InitSysCtrl();	// Device Life support & GPIO mux settings
	// init debug LED
	ledDebug.Init();
	// init Serial
	uartDBG.Init(57600);
	uartDBG.SendData("IOBoard-Grf edition v0.17\n\r");
			
// Only used if running from FLASH
#ifdef FLASH		
	// Copy time critical code and Flash setup code to RAM
	// The  RamfuncsLoadStart, RamfuncsLoadSize, and RamfuncsRunStart
	// symbols are created by the linker. Refer to the F2808.cmd file.
	//MemCopy(&RamfuncsLoadStart, &RamfuncsLoadEnd, &RamfuncsRunStart);
	memcpy(&RamfuncsRunStart, &RamfuncsLoadStart, (Uint32)&RamfuncsLoadSize);

// Call Flash Initialization to setup flash waitstates
// This function must reside in RAM
	InitFlash();	// Call the flash wrapper init function
#endif //(FLASH)

	// Timer1 INT
	EALLOW;	// This is needed to write to EALLOW protected registers
	PieVectTable.TINT1 = &CpuTimer1Isr;
	EDIS;
	IER |= M_INT13;

	// Init Components
	timer.Init();
	intTimer.Init(80, 50); // 20 kHz period
	canDrv.Init();
	ctrlMdl.initialize();
	
	// Initialize DATALOG module
	dlog.iptr1 = &dlog1;
	dlog.iptr2 = &dlog2;
	dlog.iptr3 = &dlog3;
	dlog.iptr4 = &dlog4;
	dlog.trig_value = _IQtoQ15(0.01);
	dlog.size = 200;
	dlog.prescalar = 10;
	dlog.init(&dlog);

	// Init Drivers
	ordOutputs.Init();
	pwrLEDs.Init();
	ledStrips.Init();
	rangPwrMon.Init();
	pwmDrv.Init();

	// Reset DataFile
	memset(&df, 0, sizeof(df));

	// default PWMs
	df.PWMRef1 = 1000;
	df.PWMRef2 = 1000;
	df.PWMOut1 = 1000;
	df.PWMOut2 = 1000;
	df.PWMOut3 = 1000;
	df.PWMOut4 = 1000;
	df.PWMOut5 = 1000;
	df.PWMOut6 = 1000;

	// Enable global Interrupts and higher priority real-time debug events:
	EINT;   // Enable Global interrupt INTM
	ERTM;	// Enable Global realtime interrupt DBGM

//=================================
//	Endless LOOP
//=================================
	for(;;)  //infinite loop
	{
		timer.BeginPeriod();
		df.Counter++;

		rangPwrMon.Update(); // XXX
		//df.SpeedRPM1 = rangPwrMon.m_Sonic; // SET IN CpuTimer1Isr()
		df.IRNear = rangPwrMon.m_IRNear;
		df.Force1 = rangPwrMon.m_Force;
		df.Voltage = rangPwrMon.m_Voltage;
		df.Current1 = rangPwrMon.m_Current;
		df.Power1 = df.Voltage * df.Current1;

		// check CAN RX MBOXes
		// receive
		float received;
		if( canDrv.ReadCANMBOX(0, &received) ) df.Active = (bool)received;
		if( canDrv.ReadCANMBOX(1, &received) ) df.Mode = (Uint32)received;
		if( canDrv.ReadCANMBOX(2, &received))
		{
			if(received == 1)
			{
				df.AckError = true; // must be autocleared!
				df.Active = false; // turn offline/off
				df.Mode = 0;
			}
		}

		if(canDrv.ReadCANMBOX(6, &received) )
		{
			df.PWMRef1 = received;
			df.Ping = true; // failsafe
		}
		if(canDrv.ReadCANMBOX(7, &received) ) df.PWMRef2 = received;
		//canDrv.ReadCANMBOX(8, &df.PWMOut3);
		//canDrv.ReadCANMBOX(9, &df.PWMOut4);
		//canDrv.ReadCANMBOX(10, &df.PWMOut5);
		//canDrv.ReadCANMBOX(11, &df.PWMOut6);
		//canDrv.ReadCANMBOX(12, &df.OO1);
		//canDrv.ReadCANMBOX(13, &df.OO2);
		//canDrv.ReadCANMBOX(14, &df.OO3);
		if( canDrv.ReadCANMBOX(15, &received) )
		{
			df.GyroZ = -received; // invert
		}
		canDrv.ReadCANMBOX(16, &df.OmegaRef);
		canDrv.ReadCANMBOX(17, &df.YawRef);

		canDrv.ReadCANMBOX(12, &df.MagY); // MagY
		canDrv.ReadCANMBOX(13, &df.MagZ); // MagZ

		if( canDrv.ReadCANMBOX(3, &received))
		{
			ctrlMdl.Ctrl_P.ProportionalGain_Gain = received;
			ctrlMdl.Ctrl_P.ProportionalGain_Gain_h = received;
			//ctrlMdl.Ctrl_P.ProportionalGain_Gain_j = received; // Yaw Ctrl
		}
		if( canDrv.ReadCANMBOX(4, &received))
		{
			ctrlMdl.Ctrl_P.IntegralGain_Gain = received;
			ctrlMdl.Ctrl_P.IntegralGain_Gain_d = received;
		}

		// Run controller
		ctrlMdl.Ctrl_U.PWMRef1 = df.PWMRef1;
		ctrlMdl.Ctrl_U.PWMRef2 = df.PWMRef2;
		ctrlMdl.Ctrl_U.OmegaRef = df.OmegaRef;
		ctrlMdl.Ctrl_U.YawRef = df.YawRef;
		ctrlMdl.Ctrl_U.OmegaZ = df.GyroZ;
		ctrlMdl.Ctrl_U.MagY = df.MagY;
		ctrlMdl.Ctrl_U.MagZ = df.MagZ;
		ctrlMdl.Ctrl_U.Active = df.Active;
		ctrlMdl.Ctrl_U.Mode = df.Mode;
		ctrlMdl.Ctrl_U.AckErrr = df.AckError;
		df.AckError = false; // clear ACK
		ctrlMdl.Ctrl_U.Curr1 = df.Current1;
		df.Ping = false; // clear Ping
		ctrlMdl.step();
		df.ActualState = ctrlMdl.Ctrl_Y.ActualState;
		df.ActualMode = ctrlMdl.Ctrl_Y.ActualMode;
		df.PWMOut1 = ctrlMdl.Ctrl_Y.PWM1;
		df.PWMOut2 = ctrlMdl.Ctrl_Y.PWM2;
		df.Yaw = ctrlMdl.Ctrl_Y.YawRad;
		bool Kill = ctrlMdl.Ctrl_Y.Kill;

		if( Kill )
		{
			// kill outputs
			df.PWMOut1 = 1000;
			df.PWMOut2 = 1000;
		}

		// set values
		pwmDrv.SetPWMuSec(PWMDriver::PWM1, df.PWMOut1 );
		pwmDrv.SetPWMuSec(PWMDriver::PWM2, df.PWMOut2 );
		pwmDrv.SetPWMuSec(PWMDriver::PWM3, df.PWMOut3 );
		pwmDrv.SetPWMuSec(PWMDriver::PWM4, df.PWMOut4 );
		pwmDrv.SetPWMuSec(PWMDriver::PWM5, df.PWMOut5 );
		pwmDrv.SetPWMuSec(PWMDriver::PWM6, df.PWMOut6 );

		pwrLEDs.SetDuty(PwrLEDs::L1, df.PWRLED1);
		pwrLEDs.SetDuty(PwrLEDs::L2, df.PWRLED2);
		ledStrips.SetDuty(LEDStrips::L1, df.L1Intensity);
		ledStrips.SetDuty(LEDStrips::L2, df.L2Intensity);
		ledStrips.SetDuty(LEDStrips::L3, df.L3Intensity);
		ledStrips.SetDuty(LEDStrips::L4, df.L4Intensity);	
		
		if( df.OO1 == 0) ordOutputs.Reset(OrdOutputs::O1);
		else ordOutputs.Set(OrdOutputs::O1);
		if( df.OO2 == 0) ordOutputs.Reset(OrdOutputs::O2);
		else ordOutputs.Set(OrdOutputs::O2);
		if( df.OO3 == 0) ordOutputs.Reset(OrdOutputs::O3);
		else ordOutputs.Set(OrdOutputs::O3);

		// dump all data to CAN
		// 100Hz
		canDrv.SendMBOX(20, df.SpeedRPM1);
		canDrv.SendMBOX(21, df.IRNear);
		canDrv.SendMBOX(22, df.Force1);
		canDrv.SendMBOX(23, df.Current1);
		canDrv.SendMBOX(24, df.Voltage);
		canDrv.SendMBOX(25, df.Power1);
		canDrv.SendMBOX(29, df.PWMOut1);
		canDrv.SendMBOX(30, df.PWMOut2);
		
		// 10 Hz
		if( (df.Counter % 10) == 0)
		{
			canDrv.SendMBOX(18, (float)df.ActualState);
			canDrv.SendMBOX(19, (float)df.ActualMode);
			canDrv.SendMBOX(26, df.LoopTimeMS);
			canDrv.SendMBOX(27, df.MissionTime);
			canDrv.SendMBOX(28, df.LoopTimeMSMAX);
		}
				
		// blink LED
		if( (df.Counter % 25) == 0)
		{
			ledDebug.Toggle(DebugLED::GREEN);
		}

		//-----------------------------------------------------------
		// finish loop
		df.LoopTimeMS = timer.ElapsedTime() * 1000; // get looptime (ms)
		if(df.LoopTimeMSMAX < df.LoopTimeMS) df.LoopTimeMSMAX = df.LoopTimeMS;

		while( !timer.HasElapsed(STEPTIME) )
		{
			// do something...
		};
		df.MissionTime += timer.ElapsedTime();
	}
}

// SETUP BOXES
void CANSetupBoxes(CANDriver* ptr)
{	
    // RX	
	ptr->SetupMBOX(0,  CAN_BASE+0x000100, 8, false); // Active
	ptr->SetupMBOX(1,  CAN_BASE+0x000101, 8, false); // Mode
	ptr->SetupMBOX(2,  CAN_BASE+0x000102, 8, false); // AckError
    ptr->SetupMBOX(6,  CAN_BASE+0x000120, 8, false); // PWM1
	ptr->SetupMBOX(7,  CAN_BASE+0x000121, 8, false); // PWM2
	ptr->SetupMBOX(8,  CAN_BASE+0x000122, 8, false); // PWM3
	ptr->SetupMBOX(9,  CAN_BASE+0x000123, 8, false); // PWM4
	ptr->SetupMBOX(10,  CAN_BASE+0x000124, 8, false);// PWM5
	ptr->SetupMBOX(11,  CAN_BASE+0x000125, 8, false);// PWM6
	//ptr->SetupMBOX(12,  CAN_BASE+0x000130, 8, false); // OO1
	//ptr->SetupMBOX(13,  CAN_BASE+0x000131, 8, false); // OO2
	//ptr->SetupMBOX(14,  CAN_BASE+0x000132, 8, false); // OO3
	ptr->SetupMBOX(15,  0x030000+0x000302, 8, false); // GyroZ from IMU Board
	ptr->SetupMBOX(16,  CAN_BASE+0x000140, 8, false); // OmegaRef
	ptr->SetupMBOX(17,  CAN_BASE+0x000141, 8, false); // YawRef
	
	ptr->SetupMBOX(3,  CAN_BASE+0x000150, 8, false); // PID_Pomega
	ptr->SetupMBOX(4,  CAN_BASE+0x000151, 8, false); // PID_Iomega

	ptr->SetupMBOX(12,  0x030000+0x000341, 8, false); // MagY
	ptr->SetupMBOX(13,  0x030000+0x000342, 8, false); // MagZ


	// TX
	ptr->SetupMBOX(20,  CAN_BASE+0x000300, 8, true); // sonar
	ptr->SetupMBOX(21,  CAN_BASE+0x000301, 8, true); // ir-near
	ptr->SetupMBOX(22,  CAN_BASE+0x000302, 8, true); // ir-far
	ptr->SetupMBOX(18,  CAN_BASE+0x000305, 8, true); // state real
	ptr->SetupMBOX(19,  CAN_BASE+0x000306, 8, true); // mode real
	ptr->SetupMBOX(23,  CAN_BASE+0x000310, 8, true); // current
	ptr->SetupMBOX(24,  CAN_BASE+0x000311, 8, true); // voltage
	ptr->SetupMBOX(25,  CAN_BASE+0x000312, 8, true); // power
	ptr->SetupMBOX(26,  CAN_BASE+0x000350, 8, true); // looptime
	ptr->SetupMBOX(27,  CAN_BASE+0x000351, 8, true); // totaltime
	ptr->SetupMBOX(28,  CAN_BASE+0x000352, 8, true); // totaltimeMAX

	ptr->SetupMBOX(29,  CAN_BASE+0x000315, 8, true); // PWMOut1
	ptr->SetupMBOX(30,  CAN_BASE+0x000316, 8, true); // PWMOut2
}
